/*
 * quadrotor.c
 *
 *  Created on: Jan 25, 2012
 *      Author: keith
 */
#include "sys/alt_irq.h"
#include "string.h"
#include "stdbool.h"

// define constants
// uart
#define UART_BUFFER 1024
// SPI
#define SPI_OFFSET_START 0  
#define SPI_OFFSET_READ_BYTE 1
#define SPI_OFFSET_FIFO 2
#define SPI_OFFSET_CE 3
volatile char * volatile UART_BASE = (volatile char * volatile)0x00009010;
volatile char * volatile SPI_BASE_PRX = (volatile char * volatile)0x00009008;
volatile char * volatile SPI_BASE_PTX = (volatile char * volatile)0x00009018;
int UART_IRQ = 10;
int SPI_PRX_IRQ= 9;
int SPI_PTX_IRQ = 11;
short FLAG_UART_RX_READY = 0;
short FLAG_RF_PRX_READY = 1;
short FLAG_RF_PTX_READY = 2;

// status register
char status_register = 0;

// uart_rx_buffer
char uart_rx_buffer_a[UART_BUFFER];
char uart_rx_buffer_b[UART_BUFFER];
char *uart_rx_buffer_swap_ptr = uart_rx_buffer_a;
char *uart_rx_buffer_ptr = uart_rx_buffer_a;

// rf_prx_buffer
char rf_prx_buffer[64];
// rf_ptx_buffer
char rf_ptx_buffer[64];

// function declaration
static void irq_uart_rx(void *context, alt_u32 id);
void int_to_c_str(int num, char * c);
void uart_send_str(const char const *c);
void uart_send_int(int i);
void uart_send_byte(char c);

static void irq_rf_prx_rx(void *context, alt_u32 id);
static void irq_rf_ptx_rx(void *context, alt_u32 id);
void spi_send_cmd(volatile char * volatile SPI_BASE, char *cmd, char num_cmd, char num_respond);
void spi_set_reg(volatile char * volatile SPI_BASE, char addr, char* value, char num);
void spi_read_reg(volatile char * volatile SPI_BASE, char addr, char *buffer, char bytes);
void rf_initialize_rx();
void rf_initialize_tx();
void rf_ptx_tx(const char const *payload);
void rf_prx_tx(const char const *payload);
void rf_print_fifo_info(volatile char * volatile SPI_BASE);

// interrupt handler uart rx
static void irq_uart_rx(void *context, alt_u32 id)
{
	char tmp = *UART_BASE;
	*uart_rx_buffer_swap_ptr++ = tmp;
	if (tmp == '\n')
	{
		*uart_rx_buffer_swap_ptr = 0;
		uart_rx_buffer_swap_ptr = (uart_rx_buffer_swap_ptr >= uart_rx_buffer_a) && (uart_rx_buffer_swap_ptr < uart_rx_buffer_a + UART_BUFFER) ? uart_rx_buffer_b : uart_rx_buffer_a;
		uart_rx_buffer_ptr = uart_rx_buffer_swap_ptr == uart_rx_buffer_a ? uart_rx_buffer_b : uart_rx_buffer_a;
		status_register |= 1 << FLAG_UART_RX_READY;
	}
}

// interrupt handler RF PRX
static void irq_rf_prx_rx(void *context, alt_u32 id)
{
	// disable chip;
	*(SPI_BASE_PRX + SPI_OFFSET_CE) = 0x00;

	char buffer[128];

	// read payload width
	buffer[0] = 0x60;
	spi_send_cmd(SPI_BASE_PRX, buffer,1,2);
	char payload_length = buffer[1];
	// read payload
	buffer[0] = 0x61;
	spi_send_cmd(SPI_BASE_PRX, buffer,1,payload_length+1);

	strcpy(rf_prx_buffer, &buffer[1]);

	status_register |= 1 << FLAG_RF_PRX_READY;

	// remove irq tag
	buffer[0] = 0x40;
	spi_set_reg(SPI_BASE_PRX, 0x07,buffer,1);

	// enable chip;
	*(SPI_BASE_PRX + SPI_OFFSET_CE) = 0x01;
}

static void irq_rf_ptx_rx(void *context, alt_u32 id)
{

	// save ce status
	char ce_status = *(SPI_BASE_PTX + SPI_OFFSET_CE);

	// disable chip;
	*(SPI_BASE_PTX + SPI_OFFSET_CE) = 0x00;

	char buffer[128];

	// read payload width
	buffer[0] = 0x60;
	spi_send_cmd(SPI_BASE_PTX, buffer,1,2);
	char payload_length = buffer[1];
	while (payload_length == 0)
	{
		spi_send_cmd(SPI_BASE_PTX, buffer,1,2);
		payload_length = buffer[1];
		rf_print_fifo_info(SPI_BASE_PTX);
	}
	// read payload
	buffer[0] = 0x61;
	spi_send_cmd(SPI_BASE_PTX, buffer,1,payload_length+1);

	strcpy(rf_ptx_buffer, &buffer[1]);

	status_register |= 1 << FLAG_RF_PTX_READY;

	// remove irq tag
	buffer[0] = 0x40;
	spi_set_reg(SPI_BASE_PTX, 0x07,buffer,1);

	*(SPI_BASE_PTX + SPI_OFFSET_CE) = ce_status;
}

// convert an int to a string
void int_to_c_str(int num, char * c)
{
	if (num == 0x0)
	{
		c[0]='0';
		c[1]=0;
	}
	int i = 0;
	while (num)
	{
		*(c+i++) = '0' + num % 10;
		num /= 10;
	}
	*(c+i) = 0;
	int j = 0;
	i --;
	while (j < i)
	{
		int tmp = *(c + j);
		*(c + j) = *(c+i);
		*(c+i) = tmp;
		i --;
		j ++;
	}
}

void uart_send_str(const char const *c)
{
	int i = 0;
	while (*(c+i))
	{
		*UART_BASE = *(c+i++);
	}
}

void uart_send_byte(char c)
{
	*UART_BASE = c;
}

void uart_send_int(int i)
{
	char buffer[20];
	int_to_c_str(i, buffer);
	uart_send_str(buffer);

}

void spi_send_cmd(volatile char * volatile SPI_BASE, char *cmd, char num_cmd, char num_respond)
{
	char i;
	for (i = 0; i < num_cmd; i++)
	{
		*(SPI_BASE + SPI_OFFSET_FIFO) = *(cmd + i);
	}

	*(SPI_BASE + SPI_OFFSET_READ_BYTE) = num_respond;
	*(SPI_BASE + SPI_OFFSET_START) = 0;
	char dummy;
	for (i = 0; i < num_respond; i++)
	{
		dummy = *(SPI_BASE + 0);
		*(cmd + i) = dummy;
	}
}

void spi_set_reg(volatile char * volatile SPI_BASE, char addr, char* value, char num)
{

	// transmit
	char write_addr = addr + 0x20;
	*(SPI_BASE + SPI_OFFSET_FIFO) = write_addr;

	char i;
	for (i = 0; i <num; i++)
	{
		// LSB first
		*(SPI_BASE + SPI_OFFSET_FIFO) = *(value + num - (char)1 - i);
	}
		
	*(SPI_BASE + SPI_OFFSET_READ_BYTE) = 0x01;
	*(SPI_BASE + SPI_OFFSET_START) = 0;
	char dummy = *(SPI_BASE + 0); 

	if (addr != 0x07)
	{
		// read back to check;
		*(SPI_BASE + SPI_OFFSET_FIFO) = addr;
		*(SPI_BASE + SPI_OFFSET_READ_BYTE) = num + 1;
		*(SPI_BASE + SPI_OFFSET_START) = 0;
	
		dummy = *(SPI_BASE + 0); 
		for (i = 0; i < num; i ++)
		{
			 
			dummy = *(SPI_BASE + 0); 
			if (dummy != *(value + num - 1-i))
			{
				char expected = *(value + num - 1-i);
				uart_send_str("ERROR: register not set correctly: "); 
				if (SPI_BASE == SPI_BASE_PRX)
				{
					uart_send_str ("|PRX| ");
				}
				else if (SPI_BASE == SPI_BASE_PTX)
				{
					uart_send_str ("|PTX| ");
				}
				uart_send_str("SET:|");
				uart_send_int(expected);
				uart_send_str("| GET:|");
				uart_send_int(dummy);
				uart_send_str("|\n");
			}
		}
	}
}

void spi_read_reg(volatile char * volatile SPI_BASE, char addr, char *buffer, char bytes)
{
	*(SPI_BASE + SPI_OFFSET_FIFO) = addr;
	*(SPI_BASE + SPI_OFFSET_READ_BYTE) = bytes + 1;
	*(SPI_BASE + SPI_OFFSET_START) = 0;

	*(buffer + bytes) = 0;
	
	char dummy = *(SPI_BASE);
	char i;
	for (i = 0; i < bytes; i++)
	{
		dummy = *(SPI_BASE);
		*(buffer + bytes - 1 - i) = dummy;
	}
}

void rf_print_fifo_info(volatile char * volatile SPI_BASE)
{
	char result;
	spi_read_reg(SPI_BASE, 0x17, &result, 1);

	uart_send_str("INFO: TX_FULL: ");
	if (result & 0x20)
	{
		uart_send_str("TRUE\n");
	}
	else
	{
		uart_send_str("FALSE\n");
	}

	uart_send_str("INFO: TX_EMPTY: ");
	if (result & 0x10)
	{
		uart_send_str("TRUE\n");
	}
	else
	{
		uart_send_str("FALSE\n");
	}

	uart_send_str("INFO: RX_FULL: ");
	if (result & 0x02)
	{
		uart_send_str("TRUE\n");
	}
	else
	{
		uart_send_str("FALSE\n");
	}

	uart_send_str("INFO: RX_EMPTY: ");
	if (result & 0x01)
	{
		uart_send_str("TRUE\n");
	}
	else
	{
		uart_send_str("FALSE\n");
	}
}
	
void rf_initialize_rx()
{
	char buffer[64];
	char addr, num;

	*(SPI_BASE_PRX + SPI_OFFSET_CE) = 0x01;
	// config register
	addr = 0x00;	
	buffer[0]=0x3f;
	num = 1;
	spi_set_reg(SPI_BASE_PRX, addr,buffer,num);

	// SETUP_RETER register
	addr = 0x04;	
	buffer[0]=0x13;
	num = 1;
	spi_set_reg(SPI_BASE_PRX, addr,buffer,num);


	addr = 0x07;	
	buffer[0]=0x70;
	num = 1;
	spi_set_reg(SPI_BASE_PRX, addr,buffer,num);


	// payload width
	addr = 0x11;	
	buffer[0]=0x10;
	num = 1;
	spi_set_reg(SPI_BASE_PRX, addr,buffer,num);


	// enable dynamic payload length for datalink0
	addr = 0x1c;
	buffer[0] = 0x01;
	num = 1;
	spi_set_reg(SPI_BASE_PRX, addr,buffer,num);

	// enable dynamic payload length 
	addr = 0x1d;
	buffer[0] = 0x06;
	num = 1;
	spi_set_reg(SPI_BASE_PRX, addr,buffer,num);

	// issue activate command
	buffer[0] = 0x50;
	buffer[1] = 0x73;
	spi_send_cmd(SPI_BASE_PRX, buffer,2,1);

	// issue activate command
	buffer[0] = 0xe1;
	spi_send_cmd(SPI_BASE_PRX, buffer,1,1);
	buffer[0] = 0xe2;
	spi_send_cmd(SPI_BASE_PRX, buffer,1,1);


	*(SPI_BASE_PRX + SPI_OFFSET_CE) = 0x01;
}

void rf_initialize_tx()
{
	char buffer[64];
	char addr, num;

	*(SPI_BASE_PTX + SPI_OFFSET_CE) = 0x00;

	// config register
	addr = 0x00;	
	buffer[0]=0x3e;
	num = 1;
	spi_set_reg(SPI_BASE_PTX, addr,buffer,num);

	// SETUP_RETER register
	addr = 0x04;	
	buffer[0]=0x13;
	num = 1;
	spi_set_reg(SPI_BASE_PTX, addr,buffer,num);


	addr = 0x07;	
	buffer[0]=0x70;
	num = 1;
	spi_set_reg(SPI_BASE_PTX, addr,buffer,num);


	// payload width
	addr = 0x11;	
	buffer[0]=0x10;
	num = 1;
	spi_set_reg(SPI_BASE_PTX, addr,buffer,num);


	// enable dynamic payload length for datalink0
	addr = 0x1c;
	buffer[0] = 0x01;
	num = 1;
	spi_set_reg(SPI_BASE_PTX, addr,buffer,num);

	// enable dynamic payload length 
	addr = 0x1d;
	buffer[0] = 0x06;
	num = 1;
	spi_set_reg(SPI_BASE_PTX, addr,buffer,num);


	// issue activate command
	buffer[0] = 0x50;
	buffer[1] = 0x73;
	spi_send_cmd(SPI_BASE_PTX, buffer,2,1);

	// flush rx tx fifo
	buffer[0] = 0xe1;
	spi_send_cmd(SPI_BASE_PTX, buffer,1,1);
	buffer[0] = 0xe2;
	spi_send_cmd(SPI_BASE_PTX, buffer,1,1);

	*(SPI_BASE_PTX + SPI_OFFSET_CE) = 0x00;
}

void rf_ptx_flush_tx_fifo()
{
	char buffer[10];
	// flush rx tx fifo
	buffer[0] = 0xe1;
	spi_send_cmd(SPI_BASE_PTX, buffer,1,1);
	buffer[0] = 0xe2;
	spi_send_cmd(SPI_BASE_PTX, buffer,1,1);
	buffer[0] = 0x10;

}

// send a string through tx module
// please note the string has to be c style string(null terminated)
void rf_ptx_tx(const char const * payload)
{

	char buffer[64];
	short length = strlen(payload);
	if (length >= 32)
	{
		uart_send_str("ERROR: OVER 32 BYTES, CUT TO 32 BYTES\n");
		length = 32;
	}
	buffer[0] = 0xA0;
	strcpy(&buffer[1], payload);
	spi_send_cmd(SPI_BASE_PTX, buffer,length+2,1);
	*(SPI_BASE_PTX + SPI_OFFSET_CE) = 1;

	int i;
	for (i=0; i< 60; i++)
	{
	}

	*(SPI_BASE_PTX + SPI_OFFSET_CE) = 0;

	buffer[0] = 0xff;
	spi_send_cmd(SPI_BASE_PTX,buffer,1,1);
	bool sent = buffer[0] & 0x20;
	bool failed = buffer[0] & 0x10;  
	
	while(sent == 0 && failed == 0)
	{	buffer[0] = 0xff;
		spi_send_cmd(SPI_BASE_PTX,buffer,1,1);
		sent = buffer[0] & 0x20;
		failed = buffer[0] & 0x10;  
	

		if (sent)
		{
			buffer[0] = 0x20;
			spi_set_reg(SPI_BASE_PTX, 0x07,buffer,1);
		
		}
		if (failed)
		{
			buffer[0] = 0x10;
			spi_set_reg(SPI_BASE_PTX, 0x07,buffer,1);
			rf_ptx_flush_tx_fifo();
		}
	
	}
}

void rf_prx_tx(const char const *payload)
{
	*(SPI_BASE_PRX + SPI_OFFSET_CE) = 0;

	char buffer[64];
	short length = strlen(payload);
	if (length >= 32)
	{
		uart_send_str("ERROR: OVER 32 BYTES, CUT TO 32 BYTES\n");
		length = 32;
	}
	buffer[0] = 0xA8;
	strcpy(&buffer[1], payload);
	spi_send_cmd(SPI_BASE_PRX, buffer,length+2,1);
	*(SPI_BASE_PRX + SPI_OFFSET_CE) = 1;

}


int main(void)
{
	alt_irq_register(UART_IRQ, 0, irq_uart_rx);
	alt_irq_register(SPI_PRX_IRQ, 0, irq_rf_prx_rx);
	alt_irq_register(SPI_PTX_IRQ, 0, irq_rf_ptx_rx);

	uart_send_str("INFO: Start\n");

	// initialize rf
	rf_initialize_rx();
	rf_initialize_tx();

	char local_buffer[128];

	while(1)
	{

		// check if we have received from UART
		if (status_register & (1 << FLAG_UART_RX_READY))
		{

			uart_send_str("INFO: RECEIVED: UART: ");
			strcpy(local_buffer, uart_rx_buffer_ptr);
			uart_send_str(local_buffer);

			if (strstr(local_buffer, "000:") == local_buffer)
			{
				uart_send_str(local_buffer);
			}
			else if (strstr(local_buffer, "001:") == local_buffer)
			{
				rf_prx_tx(local_buffer);
			}

			// simulate remote station
			else if (strstr(local_buffer, "100:") == local_buffer)
			{
				rf_ptx_tx(local_buffer);
			}

			else
			{
				uart_send_str("ERROR: unknown command: ");
				uart_send_str(local_buffer);
				uart_send_str("\n");
			}

			// clear flag
			status_register ^= 1 << FLAG_UART_RX_READY;
		}

		// PRX: check if we have received from PRX
		if (status_register & (1 << FLAG_RF_PRX_READY))
		{
			uart_send_str("INFO: RECEIVED: RF_PRX: ");
			strcpy(local_buffer, rf_prx_buffer);
			uart_send_str(local_buffer);

			if (strstr(local_buffer, "100:") == local_buffer)
			{
				uart_send_str(local_buffer);
			}

			else if (strstr(local_buffer, "001:") == local_buffer)
			{
				uart_send_str(local_buffer);
			}
			else
			{
				uart_send_str("ERROR: unknown command: ");
				uart_send_str(local_buffer);
				uart_send_str("\n");
			}

			// clear flag
			status_register ^= 1 << FLAG_RF_PRX_READY;

		}

		// PTX: check if we have received from PTX
		if (status_register & (1 << FLAG_RF_PTX_READY))
		{
			uart_send_str("INFO: RECEIVED: RF_PTX: ");
			strcpy(local_buffer, rf_ptx_buffer);
			uart_send_str(local_buffer);

			if (strstr(local_buffer, "001:") == local_buffer)
			{
				rf_ptx_tx(local_buffer);
			}
			else
			{
				uart_send_str("ERROR: unknown command: ");
				uart_send_str(local_buffer);
				uart_send_str("\n");
			}

		
			// clear flag
			status_register ^= 1 << FLAG_RF_PTX_READY;

		}
	}
}
